#include "../include/move.h"

// 小车
/*AAAA  ******  BBBB*/
/*1111**********1111*/
/*AAAA  ******  BBBB*/
/*        **        */
/*        **        */
/*BBBB  ******  AAAA*/
/*2222**********2222*/
/*BBBB  ******  AAAA*/

// // 左前轮A1
// #define A1_1 26
// #define A1_2 27
// // 左后轮B2
// #define B2_1 32
// #define B2_2 33
// // 右前轮B1
// #define B1_1 25
// #define B1_2 23
// // 右后轮A2
// #define A2_1 22
// #define A2_2 21

// 初始化车轮的GPIO
void Car_init(const Pin_move&pin)
{
    pinMode(pin.A1_1, OUTPUT);
    pinMode(pin.A1_2, OUTPUT);

    pinMode(pin.B2_1, OUTPUT);
    pinMode(pin.B2_2, OUTPUT);

    pinMode(pin.B1_1, OUTPUT);
    pinMode(pin.B1_2, OUTPUT);

    pinMode(pin.A2_1, OUTPUT);
    pinMode(pin.A2_2, OUTPUT);
}
// 停止 0
void Stop(const Pin_move&pin)
{
    // 左前轮  停止
    digitalWrite(pin.A1_1, LOW);
    digitalWrite(pin.A1_2, LOW);

    // 右前轮  停止
    digitalWrite(pin.B1_1, LOW);
    digitalWrite(pin.B1_2, LOW);

    // 左后轮 停止
    digitalWrite(pin.B2_1, LOW);
    digitalWrite(pin.B2_2, LOW);

    // 右后轮 停止
    digitalWrite(pin.A2_1, LOW);
    digitalWrite(pin.A2_2, LOW);
}
// 前进  3
void Go_Forward(const Pin_move&pin)
{
    // 左前轮  向前
    digitalWrite(pin.A1_1, HIGH);
    digitalWrite(pin.A1_2, LOW);

    // 右前轮  向前
    digitalWrite(pin.B1_1, LOW);
    digitalWrite(pin.B1_2, HIGH);

    // 左后轮 向前
    digitalWrite(pin.B2_1, HIGH);
    digitalWrite(pin.B2_2, LOW);

    // 右后轮 向前
    digitalWrite(pin.A2_1, LOW);
    digitalWrite(pin.A2_2, HIGH);
}
// 后退 4
void Go_Back(const Pin_move&pin)
{
    // 左前轮  向后
    digitalWrite(pin.A1_1, LOW);
    digitalWrite(pin.A1_2, HIGH);

    // 右前轮  向后
    digitalWrite(pin.B1_1, HIGH);
    digitalWrite(pin.B1_2, LOW);

    // 左后轮 向后
    digitalWrite(pin.B2_1, LOW);
    digitalWrite(pin.B2_2, HIGH);

    // 右后轮 向后
    digitalWrite(pin.A2_1, HIGH);
    digitalWrite(pin.A2_2, LOW);
}
// 正左 1
void Turn_Left(const Pin_move&pin)
{
    // 左前轮  向后
    digitalWrite(pin.A1_1, LOW);
    digitalWrite(pin.A1_2, HIGH);

    // 右前轮  向前
    digitalWrite(pin.B1_1, LOW);
    digitalWrite(pin.B1_2, HIGH);

    // 左后轮 向前
    digitalWrite(pin.B2_1, HIGH);
    digitalWrite(pin.B2_2, LOW);

    // 右后轮 向后
    digitalWrite(pin.A2_1, HIGH);
    digitalWrite(pin.A2_2, LOW);
}
// 正右 2
void Turn_Right(const Pin_move&pin)
{
    // 左前轮  向前
    digitalWrite(pin.A1_1, HIGH);
    digitalWrite(pin.A1_2, LOW);

    // 右前轮  向后
    digitalWrite(pin.B1_1, HIGH);
    digitalWrite(pin.B1_2, LOW);

    // 左后轮 向后
    digitalWrite(pin.B2_1, LOW);
    digitalWrite(pin.B2_2, HIGH);

    // 右后轮 向前
    digitalWrite(pin.A2_1, LOW);
    digitalWrite(pin.A2_2, HIGH);
}
// 原地掉头(右) 10
void U_Turn_Right(const Pin_move&pin)
{
    // 左前轮  向前
    digitalWrite(pin.A1_1, HIGH);
    digitalWrite(pin.A1_2, LOW);

    // 右前轮  向后
    digitalWrite(pin.B1_1, HIGH);
    digitalWrite(pin.B1_2, LOW);

    // 左后轮 向前
    digitalWrite(pin.B2_1, HIGH);
    digitalWrite(pin.B2_2, LOW);

    // 右后轮 向后
    digitalWrite(pin.A2_1, HIGH);
    digitalWrite(pin.A2_2, LOW);
}
// 原地掉头(左)  9
void U_Turn_Left(const Pin_move&pin)
{
    // 左前轮  向后
    digitalWrite(pin.A1_1, LOW);
    digitalWrite(pin.A1_2, HIGH);

    // 右前轮  向前
    digitalWrite(pin.B1_1, LOW);
    digitalWrite(pin.B1_2, HIGH);

    // 左后轮 向后
    digitalWrite(pin.B2_1, LOW);
    digitalWrite(pin.B2_2, HIGH);

    // 右后轮 向前
    digitalWrite(pin.A2_1, LOW);
    digitalWrite(pin.A2_2, HIGH);
}
// 右前方  6
void Right_Forward(const Pin_move&pin)
{

    // 左前轮  向前
    digitalWrite(pin.A1_1, HIGH);
    digitalWrite(pin.A1_2, LOW);

    // 右前轮  停止
    digitalWrite(pin.B1_1, LOW);
    digitalWrite(pin.B1_2, LOW);

    // 左后轮 停止
    digitalWrite(pin.B2_1, LOW);
    digitalWrite(pin.B2_2, LOW);

    // 右后轮 向前
    digitalWrite(pin.A2_1, LOW);
    digitalWrite(pin.A2_2, HIGH);
}
// 左后方 7
void Left_Back(const Pin_move&pin)
{

    // 左前轮  向后
    digitalWrite(pin.A1_1, LOW);
    digitalWrite(pin.A1_2, HIGH);

    // 右前轮  停止
    digitalWrite(pin.B1_1, LOW);
    digitalWrite(pin.B1_2, LOW);

    // 左后轮 停止
    digitalWrite(pin.B2_1, LOW);
    digitalWrite(pin.B2_2, LOW);

    // 右后轮 向后
    digitalWrite(pin.A2_1, HIGH);
    digitalWrite(pin.A2_2, LOW);
}
// 左前方 5
void Left_Forward(const Pin_move&pin)
{
    // 左前轮  停止
    digitalWrite(pin.A1_1, LOW);
    digitalWrite(pin.A1_2, LOW);

    // 右前轮  向前
    digitalWrite(pin.B1_1, LOW);
    digitalWrite(pin.B1_2, HIGH);

    // 左后轮 向前
    digitalWrite(pin.B2_1, HIGH);
    digitalWrite(pin.B2_2, LOW);

    // 右后轮 停止
    digitalWrite(pin.A2_1, LOW);
    digitalWrite(pin.A2_2, LOW);
}
// 右后方 8
void Right_Back(const Pin_move&pin)
{
    // 左前轮  停止
    digitalWrite(pin.A1_1, LOW);
    digitalWrite(pin.A1_2, HIGH);

    // 右前轮  向后
    digitalWrite(pin.B1_1, HIGH);
    digitalWrite(pin.B1_2, LOW);

    // 左后轮 向后
    digitalWrite(pin.B2_1, LOW);
    digitalWrite(pin.B2_2, HIGH);

    // 右后轮 停止
    digitalWrite(pin.A2_1, HIGH);
    digitalWrite(pin.A2_2, LOW);
}
